// - Encoder simple -

// This example simply creates an Encoder handle, hooks the event handlers, and then waits for an encoder is attached.

// Once it is attached, the program will wait for user input so that we can see the event data on the screen when using the

// encoder.

//

// Copyright 2008 Phidgets Inc.  All rights reserved.

// This work is licensed under the Creative Commons Attribution 2.5 Canada License. 

// view a copy of this license, visit http://creativecommons.org/licenses/by/2.5/ca/



#include <stdio.h>
#include <math.h>

#include <phidget21.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>

#include <encoder/coords.h>

#define PI 3.14159265

ros::Publisher odom_pub;

int CCONV AttachHandler(CPhidgetHandle ENC, void *userptr)

{

        int serialNo;

        CPhidget_DeviceID deviceID;

        int i, inputcount;



        CPhidget_getSerialNumber(ENC, &serialNo);



        //Retrieve the device ID and number of encoders so that we can set the enables if needed

        CPhidget_getDeviceID(ENC, &deviceID);

        CPhidgetEncoder_getEncoderCount((CPhidgetEncoderHandle)ENC, &inputcount);

        printf("Encoder %10d attached! \n", serialNo);



        //the 1047 requires enabling of the encoder inputs, so enable them if this is a 1047    

        if (deviceID == PHIDID_ENCODER_HS_4ENCODER_4INPUT)

        {

                printf("Encoder requires Enable. Enabling inputs....\n");

                for (i = 0 ; i < inputcount ; i++)

                        CPhidgetEncoder_setEnabled((CPhidgetEncoderHandle)ENC, i, 1);

        }

        return 0;

}





int CCONV DetachHandler(CPhidgetHandle ENC, void *userptr)

{

	int serialNo;

	CPhidget_getSerialNumber(ENC, &serialNo);

	printf("Encoder %10d detached! \n", serialNo);



	return 0;

}



int CCONV ErrorHandler(CPhidgetHandle ENC, void *userptr, int ErrorCode, const char *Description)

{

	printf("Error handled. %d - %s \n", ErrorCode, Description);



	return 0;

}



int CCONV InputChangeHandler(CPhidgetEncoderHandle ENC, void *usrptr, int Index, int State)

{

	printf("Input #%i - State: %i \n", Index, State);



	return 0;

}


// Equations: http://www.me.uwaterloo.ca/~me597/ME597-Lecture6-LocalizationI.pdf
double dl, dr, d, vl, vr, x, y, th, thp, D;


int CCONV PositionChangeHandler(CPhidgetEncoderHandle ENC, void *usrptr, int Index, int Time, int RelativePosition)

{

	int Position;

	CPhidgetEncoder_getPosition(ENC, Index, &Position);


	if(Index == 0){
		dl = -(double)(RelativePosition) / 800.0*0.048 * (double)(PI); // m
		dr = 0.0;
                vl = -(double)(RelativePosition) / 800.0*0.048 / (double)(Time) * 1000000.0; //m/s
                vr = 0.0;	
	}
	else if(Index == 1){
		dr = -(double)(RelativePosition) / 800.0*0.048 * (double)(PI); // m
		dl = 0.0;
		vr = -(double)(RelativePosition) / 800.0*0.048 / (double)(Time) * 1000000.0; //m/s
                vl = 0.0;	
	}
	
	double l = 0.3175;//0.09; // // 12.5 cm between wheels

	x = x + d * cos(th + thp/2.0);
	y = y + d * sin(th + thp/2.0);
	thp = (dr - dl) / l;
	d = (dr + dl) / 2.0;
	th = th + thp;

	D = D + d;
	/*
        if(th < 0){
	    th = th + 2*PI;
	}
	if (th >= 2*PI){
	    th = th - 2*PI;
	}
*/
	printf("x = %lf -- y = %lf -- th = %lf  -- D = %lf\n", x, y, th/PI*180.0, D);
	
	encoder::coords msg;
	msg.x = x;
	msg.y = y;
	msg.th = th/PI*180.0;
	msg.D = D;
        msg.vl = vl;
        msg.vr = vr;

	odom_pub.publish(msg);

	return 0;

}



//Display the properties of the attached phidget to the screen.  We will be displaying the name, serial number and version of the attached device.
//Will also display the number of inputs and encoders on this device

int display_properties(CPhidgetEncoderHandle phid)

{

	int serialNo, version, num_inputs, num_encoders;

	const char* ptr;



	CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);

	CPhidget_getSerialNumber((CPhidgetHandle)phid, &serialNo);

	CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);



	CPhidgetEncoder_getInputCount(phid, &num_inputs);

	CPhidgetEncoder_getEncoderCount(phid, &num_encoders);



	printf("%s\n", ptr);

	printf("Serial Number: %10d\nVersion: %8d\n", serialNo, version);

	printf("Num Encoders: %d\nNum Inputs: %d\n", num_encoders, num_inputs);



	return 0;

}



int encoder_simple()

{

	int result;

	const char *err;



	//Declare an encoder handle

	CPhidgetEncoderHandle encoder = 0;



	//create the encoder object

	CPhidgetEncoder_create(&encoder);



	//Set the handlers to be run when the device is plugged in or opened from software, unplugged or closed from software, or generates an error.

	CPhidget_set_OnAttach_Handler((CPhidgetHandle)encoder, AttachHandler, NULL);

	CPhidget_set_OnDetach_Handler((CPhidgetHandle)encoder, DetachHandler, NULL);

	CPhidget_set_OnError_Handler((CPhidgetHandle)encoder, ErrorHandler, NULL);



	//Registers a callback that will run if an input changes.

	//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).

	CPhidgetEncoder_set_OnInputChange_Handler(encoder, InputChangeHandler, NULL);



	//Registers a callback that will run if the encoder changes.

	//Requires the handle for the Encoder, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).

	CPhidgetEncoder_set_OnPositionChange_Handler (encoder, PositionChangeHandler, NULL);



	CPhidget_open((CPhidgetHandle)encoder, -1);



	//get the program to wait for an encoder device to be attached

	printf("Waiting for encoder to be attached....");

	if((result = CPhidget_waitForAttachment((CPhidgetHandle)encoder, 10000)))

	{

		CPhidget_getErrorDescription(result, &err);

		printf("Problem waiting for attachment: %s\n", err);

		return 0;

	}



	//Display the properties of the attached encoder device

	display_properties(encoder);



	//read encoder event data

	printf("Reading.....\n");



	//keep displaying encoder data until user input is read

	printf("Press any key to end\n");

	getchar();



	//since user input has been read, this is a signal to terminate the program so we will close the phidget and delete the object we created

	printf("Closing...\n");

	CPhidget_close((CPhidgetHandle)encoder);

	CPhidget_delete((CPhidgetHandle)encoder);



	//all done, exit

	return 0;

}



int main(int argc, char* argv[])

{
	ros::init(argc, argv, "encoder");
	ros::NodeHandle n;

	// http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom
	odom_pub = n.advertise<encoder::coords>("odom_pub", 1);

	dl = 0.0;
	dr = 0.0;
	d = 0.0;
	D = 0.0;

	vl = 0.0;
	vr = 0.0;
	
	x = 0.0;
	y = 0.0;
	th = PI/2.0;
	thp = 0.0;

	encoder_simple();

	return 0;

}


